/*=+--+=#=+--     Unmanned Aerial System Management Software      --+=#=+--+=#*\
|          Copyright (C) 2011 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |

     This program is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License version 2 as
     published by the Free Software Foundation.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.

            Jack Elston                       Cory Dixon                        
|           elstonj@colorado.edu              dixonc@colorado.edu              |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/

#include <stdio.h>
#include <math.h>
#include <string.h>
#include "matrix.h"
#include "ukf.h"

using namespace std;

Matrix f_roomba(Matrix, FiltData_t);
Matrix g_od(Matrix, FiltData_t);
Matrix f_bias(Matrix, FiltData_t);
Matrix g_bias(Matrix, FiltData_t);

double ang_corr;

int main()
{
  UKF filt_od, filt_b;
  FILE *logfile;
  Matrix Z("stargazer_000.txt");
  Matrix T("time000.txt");
  Matrix z_star(3,1), z_od(2,1);
  bool od_start = 1;
  double ti = T(0,0);

  ang_corr = 0;; 
  // Odometry UKF Parameters
  filt_od.param.sig_edit = 0;
  filt_od.param.alpha = 2;
  // Process Noise
  filt_od.param.Q.SetSize(6,6);
  filt_od.param.Q.Null();
  filt_od.param.Q(0,0) = 0.008;
  filt_od.param.Q(1,1) = 0.008;
  filt_od.param.Q(2,2) = 0.01;
  filt_od.param.Q(3,3) = 0.01;
  filt_od.param.Q(4,4) = 0.5*M_PI/180;
  filt_od.param.Q(5,5) = 15*M_PI/180;
  // Measurement Noise
  filt_od.param.R.SetSize(2,2);
  filt_od.param.R.Null();
  filt_od.param.R(0,0) = 0.00001;
  filt_od.param.R(1,1) = pow(0.1*M_PI/180.0,2);
  // Covariance
  filt_od.P.SetSize(6,6);
  filt_od.P.Null();
  filt_od.P(0,0) = 0.01;
  filt_od.P(1,1) = 0.01;
  filt_od.P(2,2) = 0.01;
  filt_od.P(3,3) = 0.01;
  filt_od.P(4,4) = 5*M_PI/180;
  filt_od.P(5,5) = M_PI/1800;
  // State and Measurement Functions
  filt_od.f = &f_roomba;
  filt_od.g = &g_od;
  //IC
  filt_od.x.SetSize(6,1);
  filt_od.x.Null();
  filt_od.x(0,0) = Z(0,2);
  filt_od.x(1,0) = Z(0,3);
  filt_od.x(4,0) = Z(0,4)*M_PI/180;


  // Bias UKF Parameters
  filt_b.param.sig_edit = 0;
  filt_b.param.alpha = 2;
  // Process Noise
  filt_b.param.Q.SetSize(3,3);
  filt_b.param.Q.Null();
  filt_b.param.Q(0,0) = 0.001;
  filt_b.param.Q(1,1) = 0.001;
  filt_b.param.Q(2,2) = 0.001;
  // Measurement Noise
  filt_b.param.R.SetSize(3,3);
  filt_b.param.R.Null();
  filt_b.param.R(0,0) = 0.0001;
  filt_b.param.R(1,1) = 0.0001;
  filt_b.param.R(2,2) = 0.0001;
  // Covariance
  filt_b.P.SetSize(3,3);
  filt_b.P.Null();
  filt_b.P(0,0) = 0.01;
  filt_b.P(1,1) = 0.01;
  filt_b.P(2,2) = 0.01;
  // State and Measurement Functions
  filt_b.f = &f_bias;
  filt_b.g = &g_bias;

  filt_b.x.SetSize(3,1);
  filt_b.x.Null();
  filt_b.x(0,0) = 0.0;
  filt_b.x(1,0) =  0.0;
  filt_b.x(2,0) =  0.0;

  for (size_t i=1;i<Z.NumRow();i++){
    if (Z(i,0) == 1){ // Stargazer Measurement Update
      UKF filt_t = filt_od;
	    filt_t.param.dataF[0] = T(i,0) - ti;
      filt_t.time_update();

      z_star(0,0) = Z(i,2) - filt_t.x(0,0);
      z_star(1,0) = Z(i,3) - filt_t.x(1,0);

      while (filt_t.x(4,0)*180/M_PI - (Z(i,4) + ang_corr*180/M_PI) >  300) ang_corr += 2*M_PI;
      while (filt_t.x(4,0)*180/M_PI - (Z(i,4) + ang_corr*180/M_PI) < -300) ang_corr -= 2*M_PI;
      z_star(2,0) = Z(i,4)*M_PI/180 + ang_corr - filt_t.x(4,0);

      // Check if the measurements are good or not, hack for now, FIXME
      if ( sqrt(pow(z_star(0,0),2) + pow(z_star(1,0),2)) < 0.5 )
      {
        filt_b.time_update();
        filt_b.meas_update(z_star);
      }

    }else if (Z(i,0) == 2){ // Odometry Measurement Update
      if (od_start){
        filt_od.param.dataF[0] = T(i,0) - ti;
        filt_od.time_update();
        ti = T(i,0);
        filt_od.param.dataF[1] = filt_od.x(0,0);
		    filt_od.param.dataF[2] = filt_od.x(1,0);
		    filt_od.param.dataF[3] = filt_od.x(4,0);
        od_start = 0;
      }else{
        z_od(0,0) = Z(i,8);
        z_od(1,0) = -258*Z(i,9)*M_PI/180.0;

		    filt_od.param.dataF[0] = T(i,0) - ti;
        ti = T(i,0);

        filt_od.x(0,0) += filt_b.x(0,0);
        filt_od.x(1,0) += filt_b.x(1,0);
        filt_od.x(4,0) += filt_b.x(2,0);

        filt_od.time_update();
        filt_od.meas_update(z_od);

        filt_od.param.dataF[1] = filt_od.x(0,0);
		    filt_od.param.dataF[2] = filt_od.x(1,0);
		    filt_od.param.dataF[3] = filt_od.x(4,0);

			  logfile=fopen("out.txt","a");
			  if(logfile != NULL) {
			  	fprintf(logfile,"2 %g %g %g %g %g %g %g %g %g %g\n",
			  			T(i,0),
			  			0.0,
			  			0.0,
			  			0.0,
			  			filt_od.x(0,0),
			  			filt_od.x(1,0),
			  			filt_od.x(4,0)*180/M_PI,
			  			Z(i,8),
			  			Z(i,9),
              ang_corr);
			  	fflush(logfile);
			  	fclose(logfile);
			  }
      }
    }

  }

  return 0;
}

// Roomba state transition function.
Matrix f_roomba(Matrix x, FiltData_t param){
  Matrix out(6,1);

  // Update the angle (assume constant angular velocti)
  out(4,0)  = x(4,0) + param.dataF[0]*x(5,0);
  out(5,0)  = x(5,0);

  // Update the velocity based on constant magnitude and changing heading
  out(2,0)  = sqrt( pow(x(2,0),2) + pow(x(3,0),2) ) * sin(out(4,0)); 
  out(3,0)  = sqrt( pow(x(2,0),2) + pow(x(3,0),2) ) * cos(out(4,0));

  // Update the position based on the average of velocity across the sampling time
  out(0,0)  = x(0,0) + (x(2,0) + out(2,0))/2 * param.dataF[0];
  out(1,0)  = x(1,0) + (x(3,0) + out(3,0))/2 * param.dataF[0];

  return out;
}
// Stargazer Measurement funciton.
Matrix g_od(Matrix x, FiltData_t param){
  Matrix out(2,1);
  // Sensor measures [dx,d_angle]
  out(0,0) = sqrt( pow(param.dataF[1]-x(0,0), 2) + pow(param.dataF[2]-x(1,0), 2) );
  out(1,0) = x(4,0) - param.dataF[3];
  return out;
}


// Stargazer Measurement funciton.
Matrix g_bias(Matrix x, FiltData_t param){
  return x;
}
// Stargazer Measurement funciton.
Matrix f_bias(Matrix x, FiltData_t param){
  return x;
}
